#include "time_sync.h"
#include "jitter_storage_filter.h"
#include "msg_latest_storage_strategy.h"
#include "store.h"

namespace fusion_perception {

void TimeSync::push(const std::string& frameId, ObjectSptrVector msg)
{
    if (storeUMap_.find(frameId) == storeUMap_.end()) {
        storeUMap_[frameId] = std::make_shared<Store>(
            std::make_shared<MsgLatestStorageStrategy>(int64_t(200) * 1000),
            std::make_shared<JitterStorageFilter>(3));
    }

    storeUMap_[frameId]->push(msg);
}

TimeSyncObject TimeSync::take(int64_t microTs)
{
    TimeSyncObject timeSyncObject;

    for (auto& i : storeUMap_) {
        timeSyncObject.emplace(i.first, i.second->take(microTs));
    }

    return timeSyncObject;
}

}